#include "shapeAndResponse.h"
#include <math.h>
#include "assert.h"
#include "svgDrawer.h"
#include <vector>
#include "stdio.h"
#include "stdlib.h"
#include <mex.h>
//#define SHOW_SHAPE_PROJECTION


int shapeAndResponse::sub2IndS2(int i_img, int i_orient, int i_angle, int i_length){
    // M2 map is in format of [ nOrient nLength n_image]
    int i = 0;
    i += i_img*(n_orient*n_length);
    i += i_orient*n_length;
    i += i_length;
    return i;
}


void shapeAndResponse::computeShapeScore(float **M2_maps, float *p_Mats, float *p_views, double *p_Sx, double *p_Sy, double *p_Zx, double *p_Zy,  float **score_maps, int **arg_max_map){
	
	/*
	* declare local variables	
	*/
	
    lineVertex my_line,image_line;
    line matlab_line;
    int i_page,i_pix;
    float **score_book; /*score_book for all shapes of current image (including deformations)*/
    score_book = (float**)malloc(shape_deque.size()*sizeof(float*));
    float current_score=0;
    // for each image
    for( int i_img = 0; i_img<n_img; ++i_img){

        // update image projection matrix;
        my_projector.updateCamera(&p_Mats[i_img*12],&p_views[i_img*3],p_Zx[i_img],p_Zy[i_img]);
        if (!my_projector.isVisible()) continue;
        for( int i_shape=0; i_shape<shape_deque.size();++i_shape){
            score_book[i_shape]=(float*)malloc(shape_deque[i_shape].deformation_list.size()*sizeof(float));
        }

		int deque_size = shape_deque.size();
		
        // for each shape
        for(int i_shape =0; i_shape<deque_size;++i_shape){
            float max_score = NEGMAX;
            int arg_max = -1;
            activeShape &current_shape = shape_deque[i_shape];

            for(int i_deform = 0; i_deform<current_shape.deformation_list.size();++i_deform){
                deformation &current_deformation=current_shape.deformation_list[i_deform];
                // for each deformation
#ifdef SHOW_SHAPE_PROJECTION
                char name[512];
                sprintf(name,"../cache/image_%d_shape_%d_deform%d.pshape",i_img,i_shape,i_deform);
                svgDrawer drawer(n_orient);
                const char *p_name = const_cast<const char*> (name);
                drawer.newImage(p_name,p_Sy[i_img],p_Sx[i_img]);
#endif
                current_score = 0;
                //printf("to compute shape\n");
                int i_line =0;

                if(i_shape>=shape_layer_start[1]){
                    /*this would save half of the computation time for si bian xing*/
                    int parent_shape = 	i_shape-shape_layer_start[1]+shape_layer_start[0];
                    current_score = score_book[parent_shape][i_deform];
                    i_line = 2;
                }
                for( ; i_line<current_shape.line_list.size();++i_line){

                    lineVertex &my_line = current_deformation.line_list[i_line];
                    // compute score
                    my_projector.planeLineVertex2MatlabImage(my_line, image_line);// conversion from plane line to image line, with both x axiss goes top-down, y axis goes left-right, so , image_line is not strictly following the image coordiate system we defined.
                    matlab_line=lineVertex2Line(image_line); // this is only convert the representation from struct line, to struct line2Vertex

                    if (matlab_line.length>=n_length){
                       // printf("line length greater than prepared %d\n",matlab_line.length);
                        matlab_line.length = n_length-1;
                    }
                    matlab_line.x = ROUND(matlab_line.x + 0.5f*p_Sx[i_img]+1);
                    matlab_line.y = ROUND(matlab_line.y + 0.5f*p_Sy[i_img]+1);

#ifdef SHOW_SHAPE_PROJECTION
                    lineVertex tmp_line;
                    tmp_line.x_end=image_line.y_end;
                    tmp_line.x_start=image_line.y_start;
                    tmp_line.y_end=image_line.x_end;
                    tmp_line.y_start=image_line.x_start;
                    drawer.pushLine(tmp_line);
#endif
                    // check if out of boundary
                    if(matlab_line.x<=0 || matlab_line.x>p_Sx[i_img] || matlab_line.y<=0 || matlab_line.y>p_Sy[i_img] ){
                        continue;
                    }
                    i_page=sub2IndS2(i_img,matlab_line.orient,0,matlab_line.length);
                    i_pix = matlab_line.x-1+(matlab_line.y-1)*p_Sx[i_img];

                    assert(i_pix<p_Sx[i_img]*p_Sy[i_img]);
                    assert(i_pix>=0);
                    assert(i_page>=0);
                    assert(i_page<n_length*n_orient*n_img);

                    current_score +=M2_maps[i_page][i_pix];

                }// i line

                if( max_score<=current_score){
                    max_score = current_score;
                    arg_max = i_deform;
                }
                //printf("before writting score, i_shape%d, i_deform %d\n",i_shape,i_deform);
                score_book[i_shape][i_deform]=current_score;
                //printf("after get it\n");

#ifdef SHOW_SHAPE_PROJECTION
                drawer.saveImage();
#endif
            }// deform
            score_maps[i_shape][i_img]=max_score;
            arg_max_map[i_shape][i_img]=arg_max;
        }// itr_shape

        for( int i_shape=0; i_shape<shape_deque.size();++i_shape){
            free(score_book[i_shape]);
        }
    }// i_img
    free(score_book);
}

void shapeAndResponse::initial(double *p_dim, int h){
    // input p_dim must be a (sx,sy,nImage,nOrient,nAngle,nLength);
    sx = p_dim[0];
    sy = p_dim[1];
    n_img = p_dim[2];
    n_orient = p_dim[3];
    n_angle = 0;
    n_length = p_dim[5];
    this->h = h;



    // initial rotation matrix for afffine transformation in deformation class
    this->max_rotation = 1;
    R_array = (float**)malloc(sizeof(float*)*(2*max_rotation+1));
	
    for( int d_O = -max_rotation; d_O<=max_rotation; ++d_O){
        float* R_mat=(float*)malloc(sizeof(float)*4);// 2*2 matrix, lumped by columnwise order
        float theta = d_O*PI/n_orient;
        R_mat[0]=cos(theta);  R_mat[2]=-sin(theta);
        R_mat[1] = sin(theta); R_mat[3]= cos(theta);
        R_array[d_O+max_rotation]= R_mat;

    }
    return;
}


void shapeAndResponse::release(){
    my_projector.release();
    for( int d_O = -max_rotation; d_O<=max_rotation; ++d_O){
        mxFree(R_array[d_O+max_rotation]);
    }
    mxFree(R_array);

}

void shapeAndResponse::generateShapeList(){

    generateCircle();
    generateLine();
    shape_layer_start[0]=shape_deque.size()+1;
    generateParalleLine();
    shape_layer_start[1]=shape_deque.size()+1;
    generateTrapezoids();

    // debug code, output the input shapes
#if 0
    svgDrawer drawer(this->n_orient);
    char name[100];
    for( int i_shape = 0; i_shape<this->shape_deque.size();++i_shape){
        const char* p_name = const_cast< const char *> (name);
        sprintf(name,"../cache/%06d.shape",i_shape);
        drawer.newImage(p_name,sy,sx);
        for( int i_line=0;i_line<shape_deque[i_shape].line_list.size();++i_line){
            //line  obj_line;
            //obj_line = shape_deque[i_shape].line_list[i_line];
            //obj_line.y =shape_deque[i_shape].line_list[i_line].x-sx/2;
            //obj_line.x = shape_deque[i_shape].line_list[i_line].y-sy/2;
            //drawer.pushLine(obj_line);


            lineVertex obj_line;
            obj_line.y_end = shape_deque[ind_cluster].line_list[i_line].x_end -sx/2;
            obj_line.y_start = shape_deque[ind_cluster].line_list[i_line].x_start -sx/2;

            obj_line.x_end = shape_deque[ind_cluster].line_list[i_line].y_end -sy/2;
            obj_line.x_start = shape_deque[ind_cluster].line_list[i_line].y_start -sy/2;

            drawer.pushLineVertex(obj_line);

        }
        drawer.saveImage();
    }
#endif
}

void shapeAndResponse::planeCenter(int *p_cx, int* p_cy){
    *p_cx = floor(sx*0.5f+0.5f);
    *p_cy = floor(sy*0.5f+0.5f);
}

// function to geneate circle shape and its deformation
void shapeAndResponse::generateCircle(){

    float max_dim = sx>sy? sy:sx;
    for( int i_radius = 0; i_radius<3; ++i_radius){
        float r = 0.5*max_dim*(1.0f-0.1f*(i_radius+1));
        activeShape tmp_shape;
        // shape
        tmp_shape.type = activeShape::CIRCLE;
        int num_lines = floor(2*PI*r/(1+2*h));
        float d_angle = 2*PI/num_lines;
        for( int i_line = 0; i_line<num_lines; ++i_line){
            line tmp_line;
            tmp_line.x = 0.5*sx - r*sin(2*PI/num_lines*i_line);
            tmp_line.y = 0.5*sy + r*cos(2*PI/num_lines*i_line);

            float angle = i_line*d_angle + PI/2.0f;
            int orient = floor(angle/PI*n_orient+0.5f);
            if (orient >= n_orient)
                orient = orient -n_orient;
            tmp_line.orient = orient;

            tmp_line.length = 2*h+1;
            lineVertex  tmp_line_vertex= this->line2lineVertex(tmp_line);
            tmp_shape.line_list.push_back(tmp_line_vertex);
        }

        // push all to the shape list;
        shape_deque.push_back(tmp_shape);
        pushDeformations();
    }
}


void shapeAndResponse::generateLine(){
    activeShape tmp_shape;
    int cx,cy;
    planeCenter(&cx,&cy);
    int orient = 0;
    int length = floor(sy*0.9f+0.5f);


    line tmp_line;
    tmp_line.x = cx;
    tmp_line.y = cy;
    tmp_line.orient = orient;
    tmp_line.length = length;

    lineVertex  tmp_line_vertex = this->line2lineVertex(tmp_line);
    tmp_shape.line_list.push_back(tmp_line_vertex);
    tmp_shape.type = activeShape::LINE;
    // deformation
    // no deformation, since deformation is already done in M2 map;
    // deformation
    /*
    float percent = 0.15f;
    int deform_range= floor(percent *tmp_line.length);
    int n_deform = 3; //total deformation is (2*n_deform+1)*(2*n_deform+1)
    float ddx = (float)(deform_range) / n_deform;
    float ddy = (float)(deform_range) / n_deform;

    lineVertex  tmp_deform_line_vertex;
    for( int x_deform = -n_deform; x_deform<=n_deform; ++x_deform){
        int dx = floor(x_deform*ddx+0.5f);
        for( int y_deform= -n_deform; y_deform<=n_deform; ++y_deform){
            deformation tmp_deform;
            int dy = floor(y_deform*ddy+0.5);
            tmp_deform_line_vertex.x_start = tmp_line_vertex.x_start + dx;
            tmp_deform_line_vertex.y_start= tmp_line_vertex.y_start + dy;

            tmp_deform_line_vertex.x_end = tmp_line_vertex.x_end + dx;
            tmp_deform_line_vertex.y_end = tmp_line_vertex.y_end + dy;

            tmp_deform.line_list.push_back(tmp_deform_line_vertex);
            tmp_shape.deformation_list.push_back(tmp_deform);
        }
    }

    // push line to shape list
    shape_deque.push_back(tmp_shape);
    */
    
   pushDeformations();
}

void shapeAndResponse::generateParalleLine(){
    // generate equilateral paralle line
    equiLateralLine();
    // left right angle trapozoid line
    rightAngleLeftSideLine();
    // right right angle trapozoid line
    rightAngleRightSideLine();
}


//int shapeAndResponse::pixel2GaborLength(float x){
//	int length = floor(x/(2*h+1.0f)/2.0f-1+0.5f);
//	length = MIN(n_length,length);
//	length = MAX(0,length);
//
//	return length;
//}

void shapeAndResponse::equiLateralLine(){

    float frac = 1.0f/6.0f;
    // long line at top, short line at bottom

    line tmp_line1;
    tmp_line1.x = floor(frac*1*sx);
    tmp_line1.y = floor(0.5f*sy);
    tmp_line1.orient = 0;
    tmp_line1.length = floor(sy*0.9f+0.5f);

    /*int min_length = floor(0.5f+0.4f*sy);*/
    int min_length = floor(0.5f*sy);

    line tmp_line2;
    tmp_line2.x = floor(frac*5*sx);
    tmp_line2.y = floor(0.5f*sy);
    tmp_line2.orient = 0;

    // increment the length of a line by the "gabor size"
    for( int length = min_length; length<=tmp_line1.length; length+=2*h+1){
        tmp_line2.length = length;
        pushParallelLineShape(tmp_line1,tmp_line2);
    }



    // long line at bottom, short line at top
    tmp_line1.x = floor(frac*5*sx);
    tmp_line2.x = floor(frac*sx);

    for( int length = min_length; length<=tmp_line1.length;  length+=2*h+1){
        tmp_line2.length = length;
        pushParallelLineShape(tmp_line1,tmp_line2);
    }

    return ;
}

void shapeAndResponse::rightAngleLeftSideLine(){

    float frac = 1.0f/6.0f;
    // long line at top, short line at bottom

    line tmp_line1;
    tmp_line1.x = floor(frac*1*sx);
    tmp_line1.y = floor(0.5f*sy);
    tmp_line1.orient = 0;
    tmp_line1.length = floor(0.5+sy*0.9f);

    /*int min_length = floor(0.5+0.4f*sy);*/
    int min_length = floor(0.5*sy);

    line tmp_line2;
    tmp_line2.x = floor(frac*5*sx);
    tmp_line2.orient = 0;

    for( int length = min_length; length<=tmp_line1.length;  length+=2*h+1){
        tmp_line2.length = length;
        tmp_line2.y = floor(0.5*(sy+length-tmp_line1.length));
        pushParallelLineShape(tmp_line1,tmp_line2);

    }



    // long line at bottom, short line at top
    tmp_line1.x = floor(frac*5*sx);
    tmp_line2.x = floor(frac*sx);

    for( int length = min_length; length<=tmp_line1.length;length+=2*h+1){
        tmp_line2.length = length;
        tmp_line2.y = floor(0.5*(sy+length-tmp_line1.length));
        pushParallelLineShape(tmp_line1,tmp_line2);

    }


    return;
}


void shapeAndResponse::rightAngleRightSideLine(){

    float frac = 1.0f/6.0f;
    // long line at top, short line at bottom

    line tmp_line1;
    tmp_line1.x = floor(frac*1*sx);
    tmp_line1.y = floor(0.5f*sy);
    tmp_line1.orient = 0;
    tmp_line1.length = floor(0.5f+sy*0.9f);

    //int min_length = floor(0.5f+0.4f*sy);
    int min_length = floor(0.5f*sy);

    line tmp_line2;
    tmp_line2.x = floor(frac*5*sx);
    tmp_line2.orient = 0;


    for( int length = min_length; length<=tmp_line1.length; length+=2*h+1){
        tmp_line2.length = length;
        tmp_line2.y = floor(0.5*(sy+tmp_line1.length-length));
        pushParallelLineShape(tmp_line1,tmp_line2);

    }



    // long line at bottom, short line at top
    tmp_line1.x = floor(frac*5*sx);
    tmp_line2.x = floor(frac*sx);

    for( int length = min_length; length<=tmp_line1.length; length+=2*h+1){
        tmp_line2.length = length;
        tmp_line2.y = floor(0.5*(sy+tmp_line1.length-length));
        pushParallelLineShape(tmp_line1,tmp_line2);

    }

}

void shapeAndResponse::pushDeformations(int i_shape){
    int width = this->sy;
    int height = this->sx;
    int cx = sx/2;
    int cy = sy/2;
    // push thedeformations

    // following deformation are within a +-10% *height
    int ddx = (int)(height * 0.1f);
    int ddy = (int)(width * 0.1f);
    int step_x = (int)MAX(ddx,1);
    int step_y = (int)MAX(ddy,1);
    if (i_shape ==0){
        i_shape = shape_deque.size()-1;
    }

    activeShape &shape= shape_deque[i_shape];
    shape_deque[i_shape].deformation_list.resize(0);
    lineVertex tmp_line;
    float x,y,tx,ty;// transformed x,y.
    deformation tmp_deformation;
    float scale = 1.0f;
    //for( float scale = 0.8; scale <1.21; scale = scale + 0.2){
        for( int dx = -ddx; dx <=ddx; dx+=step_x){
            for( int dy = -ddy; dy<= ddy; dy+=step_y){
                for( int d_O = -max_rotation; d_O<=max_rotation; ++d_O){
                    tmp_deformation.line_list.clear();
                    for( int i_line = 0; i_line<shape.line_list.size(); ++i_line){
                	//printf("dx %d, dy %d, scale %f, dO: %d\n",dx,dy,scale,d_O);
		        float *p_R = this->R_array[d_O+max_rotation];
                        x = shape.line_list[i_line].x_start+dx-cx;
                        y = shape.line_list[i_line].y_start+dy-cy;
                        tx=(p_R[0]*x*scale + p_R[2]*y*scale +cx); // new position, not new deformation
                        ty=(p_R[1]*x*scale + p_R[3]*y*scale +cy); // new position, not new deformation
                        tmp_line.x_start = tx;
                        tmp_line.y_start = ty;


                        x = shape.line_list[i_line].x_end+dx-cx;
                        y = shape.line_list[i_line].y_end+dy-cy;
                        tx=(p_R[0]*x*scale + p_R[2]*y*scale +cx); // new position, not new deformation
                        ty=(p_R[1]*x*scale + p_R[3]*y*scale +cy); // new position, not new deformation
                        tmp_line.x_end = tx;
                        tmp_line.y_end = ty;

                        tmp_deformation.line_list.push_back(tmp_line);
                    }// i_line
                   shape_deque[i_shape].deformation_list.push_back(tmp_deformation);
                }// d_Orient
            }// dx
        }// dy
    //}// scale
    //int n_deform = shape_deque[i_shape].deformation_list.size();
    //printf(" width : %d, height %d, deformations %d\n",width,height,n_deform);
}



void shapeAndResponse::pushParallelLineShape(line &line1, line &line2){
    activeShape tmp_shape;

    lineVertex line_vertex_1,line_vertex_2;

    line_vertex_1 = this->line2lineVertex(line1);
    line_vertex_2 = this->line2lineVertex(line2);

    tmp_shape.line_list.push_back(line_vertex_1);
    tmp_shape.line_list.push_back(line_vertex_2);

    tmp_shape.type = activeShape::PARA_LINE;

    shape_deque.push_back(tmp_shape);
    this->pushDeformations();
}

void shapeAndResponse::generateTrapezoids(){
    // Get the pointer to last element
    int i_shape = shape_deque.size();

    lineVertex line3,line4;
    generateParalleLine();
    // push new elements
    for( ;i_shape<shape_deque.size();++i_shape){
        // create new lines to make them a closed shape.
        shape_deque[i_shape].type= activeShape::TRAPEZOIDS;

        lineVertex &line1= shape_deque[i_shape].line_list.front();
        lineVertex &line2 = shape_deque[i_shape].line_list.back();

        line3.x_start = line1.x_start;
        line3.y_start = line1.y_start;
        line3.x_end = line2.x_start;
        line3.y_end = line2.y_start;

        line4.x_start = line1.x_end;
        line4.y_start = line1.y_end;
        line4.x_end = line2.x_end;
        line4.y_end = line2.y_end;


        shape_deque[i_shape].line_list.push_back(line3);
        shape_deque[i_shape].line_list.push_back(line4);

        this->pushDeformations(i_shape);

    }// each shape
}


void shapeAndResponse::drawShapeInCanonicalView(int *p_cluster_ind, const char *p_out_path){

    std::list<int> clusters;
    for( int i = 0; i< n_img;++i){
        clusters.push_back(p_cluster_ind[i]);
    }
    clusters.unique();

    while(!clusters.empty()){
        int ind_cluster = clusters.back()-1;
        clusters.pop_back();

        svgDrawer drawer(this->n_orient);
        char name[256];
        const char* p_name = const_cast< const char *> (name);
        sprintf(name,"%s/canonical_%d.shape",p_out_path,ind_cluster);
        drawer.newImage(p_name,sy,sx);
        for( int i_line=0;i_line<shape_deque[ind_cluster].line_list.size();++i_line){
            lineVertex  obj_line;
            obj_line = shape_deque[ind_cluster].line_list[i_line];
            //obj_line.y =shape_deque[ind_cluster].line_list[i_line].x-sx/2;
            //obj_line.x = shape_deque[ind_cluster].line_list[i_line].y-sy/2;

            obj_line.y_end = shape_deque[ind_cluster].line_list[i_line].x_end -sx/2;
            obj_line.y_start = shape_deque[ind_cluster].line_list[i_line].x_start -sx/2;

            obj_line.x_end = shape_deque[ind_cluster].line_list[i_line].y_end -sy/2;
            obj_line.x_start = shape_deque[ind_cluster].line_list[i_line].y_start -sy/2;

            drawer.pushLineVertex(obj_line);
        }
        drawer.saveImage();
    }
}


void shapeAndResponse::drawShapeInDeformedView(float *p_Mats, float *p_views, double *p_Sx, double *p_Sy, double *p_Zx, double *p_Zy,
                                               int *p_cluster_ind, int *p_arg_max_map, const char *p_out_path, double *p_if_plot){
    printf("drawing shape for %s\n",p_out_path);
    // for each image
    for( int i_img = 0; i_img<n_img; ++i_img){
        int i_shape = p_cluster_ind[i_img]-1;
        int i_deform = p_arg_max_map[i_shape*n_img+i_img];
        if(i_deform<0) continue;
        if ( p_if_plot[i_img]< 0.5)
            continue;

        // update image projection matrix;
        my_projector.updateCamera(&p_Mats[i_img*12],&p_views[i_img*3],p_Zx[i_img],p_Zy[i_img]);
        if (!my_projector.isVisible()) continue;
        // for specified shape and deformation
        printf("drawing for image %d, shape %d, deform %d\n",i_img,i_shape,i_deform);
        char name[512];
        sprintf(name,"%s/image_%d_shape_%d_deform%d.pshape",p_out_path,i_img,i_shape,i_deform);
        svgDrawer drawer(n_orient);
        const char *p_name = const_cast<const char*> (name);
        drawer.newImage(p_name,p_Sy[i_img],p_Sx[i_img]);

        float current_score = 0;
        for( int i_line = 0; i_line<shape_deque[i_shape].line_list.size();++i_line){

            lineVertex &my_line = shape_deque[i_shape].deformation_list[i_deform].line_list[i_line];

            // compute score
            lineVertex image_line;
            my_projector.planeLineVertex2MatlabImage(my_line, image_line);

            lineVertex tmp_line;
            tmp_line.x_end=image_line.y_end;
            tmp_line.x_start=image_line.y_start;
            tmp_line.y_end=image_line.x_end;
            tmp_line.y_start=image_line.x_start;
            drawer.pushLineVertex(tmp_line);
        }// i line
        drawer.saveImage();
    }// i_img
}

line shapeAndResponse::lineVertex2Line(const lineVertex &source){
    float cx,cy,dx,dy;
    line res_line;

    cx  =( source.x_end+source.x_start)*0.5f;
    cy = (source.y_start+source.y_end)*0.5f;
    dx = (source.x_end-source.x_start);
    dy = (source.y_end-source.y_start);
    // assume in plane coordinate
    float theta = atan(-dx/dy);
    if(fabs(dy)<1e-8){
        theta= PI/2;
    }

    int orient = floor(theta/PI*n_orient+0.5);
    if(orient<0)
        orient+=n_orient;

    float length = sqrt(dx*dx+dy*dy);
    //length  =floor((length/(2*h+1)-1)/2+0.5);
    length = floor(length/(4*h+2));

    res_line.length = MAX(0,length);
    res_line.x = cx;
    res_line.y = cy;
    res_line.orient = orient;

    return res_line;
}

lineVertex shapeAndResponse::line2lineVertex(const line &source){
    lineVertex res_line_vertex;
    //float half_length = (source.length*2+1)*(2*h+1)*0.5f;
    float half_length = 0.5f * source.length;

    float theta = PI*source.orient/n_orient;
    float pre_sin = sin(theta);
    float pre_cos = cos(theta);

    res_line_vertex.x_start = source.x - half_length*pre_sin;
    res_line_vertex.y_start = source.y + half_length*pre_cos;

    res_line_vertex.x_end = source.x + half_length*pre_sin;
    res_line_vertex.y_end = source.y - half_length*pre_cos;

    return res_line_vertex;
}
